import os
from pathlib import Path

import launch_ros.actions
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory('turn_on_bichon')
    launch_dir = os.path.join(bringup_dir, 'launch')
    include_dir = os.path.join(launch_dir, 'include')
    imu_config = Path(get_package_share_directory('turn_on_bichon'),'config', 'imu.yaml')

    carto_slam = LaunchConfiguration('carto_slam', default='false')

    carto_slam_dec = DeclareLaunchArgument('carto_slam', default_value='false')

    bichon_robot = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(include_dir, 'base_serial_h30imu.launch.py')),
            launch_arguments={'use_bichon_imu':'true'}.items(),
    )

    robot_ekf = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bichon_ekf.launch.py')),
            launch_arguments={'carto_slam':carto_slam}.items(),
    )

    base_to_link = launch_ros.actions.Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='base_to_link',
            arguments=['0', '0', '0','0', '0','0','base_footprint','base_link'],
    )

    base_to_gyro = launch_ros.actions.Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='base_to_gyro',
            arguments=['0', '0', '0','0', '0','0','base_footprint','gyro_link'],
    )

    imu_filter_node =  launch_ros.actions.Node(
        package='imu_filter_madgwick',
        executable='imu_filter_madgwick_node',
        parameters=[imu_config]
    )

    foxglove_bridge_node = launch_ros.actions.Node(
        package='foxglove_bridge',
        executable='foxglove_bridge',
        name='foxglove_bridge',
        )

    intel_realsense_node = launch_ros.actions.Node(
            package='realsense2_camera',
            executable='realsense2_camera_node',
            name='realsense2_camera',
    )

    joint_state_publisher_node = launch_ros.actions.Node(
            package='joint_state_publisher',
            executable='joint_state_publisher',
            name='joint_state_publisher',
    )

    robot_mode_description = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description_minibot.launch.py')),
            launch_arguments={'mini_akm': 'true'}.items(),
    )

    lslidar_dir = get_package_share_directory('lslidar_driver')
    lslidar_launch_dir = os.path.join(lslidar_dir, 'launch')

    lsn10 = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(
            lslidar_launch_dir, 'lsn10_launch.py')),
    )

    ld = LaunchDescription()

    ld.add_action(robot_mode_description)
    ld.add_action(carto_slam_dec)
    ld.add_action(bichon_robot)
    ld.add_action(base_to_link)
    ld.add_action(base_to_gyro)
    ld.add_action(robot_ekf)

    ld.add_action(joint_state_publisher_node)
    ld.add_action(imu_filter_node)
    ld.add_action(foxglove_bridge_node)
    ld.add_action(intel_realsense_node)
    ld.add_action(lsn10)

    return ld
